/*
 #
 #  Files       : SquareMatrix.h
 #                ( C++ header file )
 #
 #  Description : The SmallMatrix Library
 #                ( http://code.google.com/p/smallmatrix )
 #
 #  Copyright   : Olivier Juan
 #                ( http://www.mas.ecp.fr/vision/Personnel/juan/ )
 #
 #  License     : CeCILL-C
 #                ( http://www.cecill.info/licences/Licence_CeCILL-C_V1-en.html )
 #
 #  This software is governed by the CeCILL-C license under French law and
 #  abiding by the rules of distribution of free software.  You can  use,
 #  modify and or redistribute the software under the terms of the CeCILL-C
 #  license as circulated by CEA, CNRS and INRIA at the following URL
 #  "http://www.cecill.info".
 #
 #  As a counterpart to the access to the source code and  rights to copy,
 #  modify and redistribute granted by the license, users are provided only
 #  with a limited warranty  and the software's author,  the holder of the
 #  economic rights,  and the successive licensors  have only  limited
 #  liability.
 #
 #  In this respect, the user's attention is drawn to the risks associated
 #  with loading,  using,  modifying and/or developing or reproducing the
 #  software by the user in light of its specific status of free software,
 #  that may mean  that it is complicated to manipulate,  and  that  also
 #  therefore means  that it is reserved for developers  and  experienced
 #  professionals having in-depth computer knowledge. Users are therefore
 #  encouraged to load and test the software's suitability as regards their
 #  requirements in conditions enabling the security of their systems and/or
 #  data to be ensured and,  more generally, to use and operate it in the
 #  same conditions as regards security.
 #
 #  The fact that you are presently reading this means that you have had
 #  knowledge of the CeCILL-C license and that you accept its terms.
 #
*/

#ifndef _SQUAREMATRIX_H
#define _SQUAREMATRIX_H

#include <SmallMatrix/SmallMatrix.h>

NAMESPACE_BEGIN(SmallMatrix)

template <int N> class SquareMatrix: public Matrix_Base<SquareMatrix<N>,N*N,N,N>
{
public:
	SquareMatrix():Matrix_Base<SquareMatrix<N>,N*N,N,N>(){}
	SquareMatrix(const reel &v):Matrix_Base<SquareMatrix<N>,N*N,N,N>(v){}
	SquareMatrix(const reel tab[N*N]):Matrix_Base<SquareMatrix<N>,N*N,N,N>(tab){}
	template <typename TT,int SSIZE> SquareMatrix(const Matrix_Base<TT,SSIZE,N,N>& copy):Matrix_Base<SquareMatrix<N>,N*N,N,N>(copy){}
	inline reel& operator()(const int& idxm,const int& idxn);
	inline const reel& operator()(const int& idxm,const int& idxn)const;
	template <typename T1,int SIZE1,int MN,typename T2,int SIZE2> inline SquareMatrix<N>& prod(const Matrix_Base<T1,SIZE1,N,MN>& M1,const Matrix_Base<T2,SIZE2,MN,N>& M2);
	template <typename T1,int SIZE1,typename T2,int SIZE2> inline SquareMatrix<N>& add(const Matrix_Base<T1,SIZE1,N,N>& M1,const Matrix_Base<T2,SIZE2,N,N>& M2);
	template <typename T1,int SIZE1,typename T2,int SIZE2> inline SquareMatrix<N>& subst(const Matrix_Base<T1,SIZE1,N,N>& M1,const Matrix_Base<T2,SIZE2,N,N>& M2);
	template <typename T1,int SIZE1> inline SquareMatrix<N>& add(const Matrix_Base<T1,SIZE1,N,N>& M1);
	template <typename T1,int SIZE1> inline SquareMatrix<N>& subst(const Matrix_Base<T1,SIZE1,N,N>& M1);
	template <typename TT,int SSIZE> inline SquareMatrix<N>& mult(const Matrix_Base<TT,SSIZE,N,N>& Two);
	template <typename TT,int SSIZE> inline SquareMatrix<N>& div(const Matrix_Base<TT,SSIZE,N,N>& Two);
	inline SquareMatrix<N> trans()const;
	inline reel trace()const;
	inline SquareMatrix<N> inv()const;
	inline SquareMatrix<N> inv(reel& determ)const;
	inline reel det()const;
	inline SquareMatrix<N>& adddiag(const reel& x);
};
//! Read/Write Accessor
template <int N> inline reel& SquareMatrix<N>::operator()(const int& idxm,const int& idxn)
{
	assert((idxm>=0)&&(idxm<N)&&(idxn>=0)&&(idxn<N));
	SquareMatrix<N>& ref=*this;
	return ref[idxm*N+idxn];
}
//! Read/Only Accessor
template <int N> inline const reel& SquareMatrix<N>::operator()(const int& idxm,const int& idxn) const
{
	assert((idxm>=0)&&(idxm<N)&&(idxn>=0)&&(idxn<N));
	const SquareMatrix<N>& ref=*this;
	return ref[idxm*N+idxn];
}
//! Matrix-Matrix Product in which the result is a SquareMatrix
template <int N> template <typename T1,int SIZE1,int MN,typename T2,int SIZE2> inline  SquareMatrix<N>& SquareMatrix<N>::prod(const Matrix_Base<T1,SIZE1,N,MN>& M1,const Matrix_Base<T2,SIZE2,MN,N>& M2) {
	SquareMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=0;j<N;j++) {
			ref(i,j)=0;
			for (int k=0;k<MN;k++)
				ref(i,j)+=M1(i,k)*M2(k,j);
		}
	return ref;
}
//! Matrix-Matrix Sum in which the result is a SquareMatrix
template <int N> template <typename T1,int SIZE1,typename T2,int SIZE2> inline SquareMatrix<N>& SquareMatrix<N>::add(const Matrix_Base<T1,SIZE1,N,N>& M1,const Matrix_Base<T2,SIZE2,N,N>& M2) {
	SquareMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=0;j<N;j++)
			ref(i,j)=M1(i,j)+M2(i,j);
	return ref;
}
//! Matrix-Matrix Substract in which the result is a SquareMatrix
template <int N> template <typename T1,int SIZE1,typename T2,int SIZE2> inline SquareMatrix<N>& SquareMatrix<N>::subst(const Matrix_Base<T1,SIZE1,N,N>& M1,const Matrix_Base<T2,SIZE2,N,N>& M2) {
	SquareMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=0;j<N;j++)
			ref(i,j)=M1(i,j)-M2(i,j);
	return ref;
}
//! Matrix-Matrix Sum in which the result is a SquareMatrix
template <int N> template <typename T1,int SIZE1> inline SquareMatrix<N>& SquareMatrix<N>::add(const Matrix_Base<T1,SIZE1,N,N>& M1) {
	SquareMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=0;j<N;j++)
			ref(i,j)+=M1(i,j);
	return ref;
}
//! Matrix-Matrix Substract in which the result is a SquareMatrix
template <int N> template <typename T1,int SIZE1> inline SquareMatrix<N>& SquareMatrix<N>::subst(const Matrix_Base<T1,SIZE1,N,N>& M1) {
	SquareMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=0;j<N;j++)
			ref(i,j)-=M1(i,j);
	return ref;
}
template <int N> template <typename TT,int SSIZE> inline SquareMatrix<N>& SquareMatrix<N>::mult(const Matrix_Base<TT,SSIZE,N,N>& Two) {
	SquareMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=0;j<N;j++)
			ref(i,j)*=Two(i,j);
	return ref;
}
template <int N> template <typename TT,int SSIZE> inline SquareMatrix<N>& SquareMatrix<N>::div(const Matrix_Base<TT,SSIZE,N,N>& Two) {
	SquareMatrix<N>& ref=*this;
	for (int i=0;i<N;i++)
		for (int j=0;j<N;j++) {
			assert(Two(i,j)!=0);
			ref(i,j)/=Two(i,j);
		}
	return ref;
}
//! SquareMatrix Transpose
template <int N> inline SquareMatrix<N> SquareMatrix<N>::trans()const
{
	const SquareMatrix<N>& ref=*this;
	SquareMatrix<N> res;
	for (int i=0;i<N;i++)
		for (int j=0;j<N;j++)
			res(j,i)=ref(i,j);
	return res;
}
//! SquareMatrix Trace
template <int N> inline reel SquareMatrix<N>::trace()const
{
	const SquareMatrix<N>& ref=*this;
	reel res(0);
	for (int i=0;i<N;i++)
		res+=ref(i,i);
	return res;
}
//! SquareMatrix Inverse
template <> inline SquareMatrix<1> SquareMatrix<1>::inv()const
{
	const SquareMatrix<1>& ref=*this;
	SquareMatrix<1> res;
	assert(ref(0,0)!=0);
	res(0,0)=1/ref(0,0);
	return res;
}
template <> inline SquareMatrix<2> SquareMatrix<2>::inv()const
{
	const SquareMatrix<2>& ref=*this;
	reel determ=ref(0,0)*ref(1,1)-ref(1,0)*ref(0,1);
	assert(determ!=0);
	SquareMatrix<2> res;
	res(0,0)=ref(1,1)/determ;
	res(0,1)=-ref(0,1)/determ;
	res(1,0)=-ref(0,1)/determ;
	res(1,1)=ref(0,0)/determ;
	return res;
}
template <> inline SquareMatrix<3> SquareMatrix<3>::inv()const
{
	reel determ;
	const SquareMatrix<3>& ref=*this;
	reel d0=ref(1,1)*ref(2,2)-ref(1,2)*ref(2,1);
	reel d1=ref(1,0)*ref(2,2)-ref(2,0)*ref(1,2);
	reel d2=ref(1,0)*ref(2,1)-ref(1,1)*ref(2,0);
	determ=ref(0,0)*d0-ref(0,1)*d1+ref(0,2)*d2;
	assert(determ!=0);
	reel d=reel(1)/determ;
	reel d3=ref(0,1)*ref(2,2)-ref(2,1)*ref(0,2);
	reel d4=ref(0,0)*ref(2,2)-ref(2,0)*ref(0,2);
	reel d5=ref(0,0)*ref(2,1)-ref(2,0)*ref(0,1);
	reel d6=ref(0,1)*ref(1,2)-ref(1,1)*ref(0,2);
	reel d7=ref(0,0)*ref(1,2)-ref(1,0)*ref(0,2);
	reel d8=ref(0,0)*ref(1,1)-ref(1,0)*ref(0,1);
	SquareMatrix<3> res;
	res(0,0)=d0*d;
	res(0,1)=-d3*d;
	res(0,2)=d6*d;
	res(1,0)=-d1*d;
	res(1,1)=d4*d;
	res(1,2)=-d7*d;
	res(2,0)=d2*d;
	res(2,1)=-d5*d;
	res(2,2)=d8*d;
	return res;
}
template <> inline SquareMatrix<4> SquareMatrix<4>::inv()const
{
	const SquareMatrix<4>& ref=*this;
	reel d1201=ref(1,0)*ref(2,1)-ref(1,1)*ref(2,0);
	reel d1202=ref(1,0)*ref(2,2)-ref(1,2)*ref(2,0);
	reel d1203=ref(1,0)*ref(2,3)-ref(1,3)*ref(2,0);
	reel d1212=ref(1,1)*ref(2,2)-ref(1,2)*ref(2,1);
	reel d1213=ref(1,1)*ref(2,3)-ref(1,3)*ref(2,1);
	reel d1223=ref(1,2)*ref(2,3)-ref(1,3)*ref(2,2);
	reel d1301=ref(1,0)*ref(3,1)-ref(1,1)*ref(3,0);
	reel d1302=ref(1,0)*ref(3,2)-ref(1,2)*ref(3,0);
	reel d1303=ref(1,0)*ref(3,3)-ref(1,3)*ref(3,0);
	reel d1312=ref(1,1)*ref(3,2)-ref(1,2)*ref(3,1);
	reel d1313=ref(1,1)*ref(3,3)-ref(1,3)*ref(3,1);
	reel d1323=ref(1,2)*ref(3,3)-ref(1,3)*ref(3,2);
	reel d2301=ref(2,0)*ref(3,1)-ref(2,1)*ref(3,0);
	reel d2302=ref(2,0)*ref(3,2)-ref(2,2)*ref(3,0);
	reel d2303=ref(2,0)*ref(3,3)-ref(2,3)*ref(3,0);
	reel d2312=ref(2,1)*ref(3,2)-ref(2,2)*ref(3,1);
	reel d2313=ref(2,1)*ref(3,3)-ref(2,3)*ref(3,1);
	reel d2323=ref(2,2)*ref(3,3)-ref(2,3)*ref(3,2);
	SquareMatrix<4> res;
	res(0,0)=(ref(1,1)*d2323-ref(1,2)*d2313+ref(1,3)*d2312);
	res(1,0)=-(ref(1,0)*d2323-ref(1,2)*d2303+ref(1,3)*d2302);
	res(2,0)=(ref(1,0)*d2313-ref(1,1)*d2303+ref(1,3)*d2301);
	res(3,0)=-(ref(1,0)*d2312-ref(1,1)*d2302+ref(1,2)*d2301);
	res(0,1)=-(ref(0,1)*d2323-ref(0,2)*d2313+ref(0,3)*d2312);
	res(1,1)=(ref(0,0)*d2323-ref(0,2)*d2303+ref(0,3)*d2302);
	res(2,1)=-(ref(0,0)*d2313-ref(0,1)*d2303+ref(0,3)*d2301);
	res(3,1)=(ref(0,0)*d2312-ref(0,1)*d2302+ref(0,2)*d2301);
	res(0,2)=(ref(0,1)*d1323-ref(0,2)*d1313+ref(0,3)*d1312);
	res(1,2)=-(ref(0,0)*d1323-ref(0,2)*d1303+ref(0,3)*d1302);
	res(2,2)=(ref(0,0)*d1313-ref(0,1)*d1303+ref(0,3)*d1301);
	res(3,2)=-(ref(0,0)*d1312-ref(0,1)*d1302+ref(0,2)*d1301);
	res(0,3)=-(ref(0,1)*d1223-ref(0,2)*d1213+ref(0,3)*d1212);
	res(1,3)=(ref(0,0)*d1223-ref(0,2)*d1203+ref(0,3)*d1202);
	res(2,3)=-(ref(0,0)*d1213-ref(0,1)*d1203+ref(0,3)*d1201);
	res(3,3)=(ref(0,0)*d1212-ref(0,1)*d1202+ref(0,2)*d1201);
	reel determ=ref(0,0)*res(0,0)+ref(0,1)*res(1,0)+ref(0,2)*res(2,0)+ref(0,3)*res(3,0);
	assert(determ!=0);
	for (int i=0; i<4; i++)
		for (int j=0; j<4; j++)
			res(i,j)/=determ;
	return res;
}
template <int N> inline SquareMatrix<N> SquareMatrix<N>::inv()const
{
	std::cerr<<"matrice nxn : inverse non defini seulement pour n>4 !!!"<<std::endl;
	return this; // une erreur sera generee si cette fonction est compilee
}
template <> inline SquareMatrix<3> SquareMatrix<3>::inv(reel& determ)const
{
	const SquareMatrix<3>& ref=*this;
	reel d0=ref(1,1)*ref(2,2)-ref(1,2)*ref(2,1);
	reel d1=ref(1,0)*ref(2,2)-ref(2,0)*ref(1,2);
	reel d2=ref(1,0)*ref(2,1)-ref(1,1)*ref(2,0);
	determ=ref(0,0)*d0-ref(0,1)*d1+ref(0,2)*d2;
	assert(determ!=0);
	reel d=reel(1)/determ;
	reel d3=ref(0,1)*ref(2,2)-ref(2,1)*ref(0,2);
	reel d4=ref(0,0)*ref(2,2)-ref(2,0)*ref(0,2);
	reel d5=ref(0,0)*ref(2,1)-ref(2,0)*ref(0,1);
	reel d6=ref(0,1)*ref(1,2)-ref(1,1)*ref(0,2);
	reel d7=ref(0,0)*ref(1,2)-ref(1,0)*ref(0,2);
	reel d8=ref(0,0)*ref(1,1)-ref(1,0)*ref(0,1);
	SquareMatrix<3> res;
	res(0,0)=d0*d;
	res(0,1)=-d3*d;
	res(0,2)=d6*d;
	res(1,0)=-d1*d;
	res(1,1)=d4*d;
	res(1,2)=-d7*d;
	res(2,0)=d2*d;
	res(2,1)=-d5*d;
	res(2,2)=d8*d;
	return res;
}
template <int N> inline SquareMatrix<N> SquareMatrix<N>::inv(reel& determ)const
{
	std::cerr<<"matrice non 3x3 !!!"<<std::endl;
	return this; // une erreur sera generee si cette fonction est compilee
}
//! SquareMatrix Determinant
template <> inline reel SquareMatrix<1>::det()const
{
	const SquareMatrix<1>& ref=*this;
	return ref(0,0);
}
template <> inline reel SquareMatrix<2>::det()const
{
	const SquareMatrix<2>& ref=*this;
	return ref(0,0)*ref(1,1)-ref(0,1)*ref(1,0);
}
template <> inline reel SquareMatrix<3>::det()const
{
	const SquareMatrix<3>& ref=*this;
	reel d0=ref(1,1)*ref(2,2)-ref(1,2)*ref(2,1);
	reel d1=ref(1,0)*ref(2,2)-ref(1,2)*ref(2,0);
	reel d2=ref(1,0)*ref(2,1)-ref(1,1)*ref(2,0);
	return ref(0,0)*d0-ref(0,1)*d1+ref(0,2)*d2;
}
template <> inline reel SquareMatrix<4>::det()const
{
	const SquareMatrix<4>& ref=*this;
	reel d01=ref(2,2)*ref(3,3)-ref(2,3)*ref(3,2);
	reel d02=ref(2,1)*ref(3,3)-ref(2,3)*ref(3,1);
	reel d03=ref(2,1)*ref(3,2)-ref(2,2)*ref(3,1);
	reel d12=ref(2,0)*ref(3,3)-ref(2,3)*ref(3,0);
	reel d13=ref(2,0)*ref(3,2)-ref(2,2)*ref(3,0);
	reel d23=ref(2,0)*ref(3,1)-ref(2,1)*ref(3,0);
	reel d0=ref(1,1)*d01-ref(1,2)*d02+ref(1,3)*d03;
	reel d1=ref(1,0)*d01-ref(1,2)*d12+ref(1,3)*d13;
	reel d2=ref(1,0)*d02-ref(1,1)*d12+ref(1,3)*d23;
	reel d3=ref(1,0)*d03-ref(1,1)*d13+ref(1,2)*d23;
	return ref(0,0)*d0-ref(0,1)*d1+ref(0,2)*d2-ref(0,3)*d3;
}
template <int N> inline reel SquareMatrix<N>::det()const
{
	std::cerr<<"matrice nxn : determinant non defini seulement pour n>4 !!!"<<std::endl;
	return this; // une erreur sera generee si cette fonction est compilee
}
//! Add \a x on the Diagonal of the SquareMatrix
template <int N> inline SquareMatrix<N>& SquareMatrix<N>::adddiag(const reel& x)
{
	SquareMatrix& ref=*this;
	for (int i=0;i<N;i++)
		ref(i,i)+=x;
	return ref;
}


NAMESPACE_END

#endif
